import io
import socket
import struct
import cv2
import sys
import numpy as np
from getL import getL
from detect_laser import detect_laser
import math
import time

params = [cv2.IMWRITE_JPEG_QUALITY, 50]

ip_filename = "/home/pi/workspace/stereo-cv/config/ip.txt"
ipServer = ""
with open(ip_filename, "r") as f:  # 打开文件
    data = f.readline()  # 读取文件
    datas = data.split(" ")
    ipServer = datas[1].split("\n")[0]
    f.close()

client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
client_socket.connect((ipServer, 8000))

cap = cv2.VideoCapture(0)
height = 720
width = 1280
cap.set(3,width)
cap.set(4,height)

cnt = 0
# 忽略前几帧
while(cnt < 10):
    cnt += 1
    flag, frame  = cap.read()
    detect_laser(frame)

# 获取ROI
center = [0, 0]
N = 10
cnt = 0
CNT = 0
while(cnt < N):
    cnt += 1
    flag, frame  = cap.read()
    result = detect_laser(frame)
    if result != 0:
        CNT += 1
        center[0] += result[0]
        center[1] += result[1]
center[0] = round(center[0] / CNT)
center[1] = round(center[1] / CNT)
winx = round(width / 640 * 120)
winy = round(height / 480 * 40)
# x1 x2 y1 y2
roi = [center[0] - winx, center[0] + winx, center[1] - winy, center[1] + winy]

cnt = 0
resultList = []
startTime = time.time()
while cnt <= 200:
    cnt += 1
    flag, frame  = cap.read()
    frame = frame[roi[0] : roi[1], roi[2] : roi[3]]
    timestamp = time.time()

    result = detect_laser(frame)
    if(result != 0):
        result[0] += roi[0]
        result[1] += roi[2]
        result.append(timestamp)
        print(result)
        resultList.append(result)

    img_encode = cv2.imencode('.jpg', frame, params)[1]
    bytedata = img_encode.tostring()
    
    flag_data = (str(len(bytedata))).encode() + ",".encode() + " ".encode()
    client_socket.send(flag_data)
    data = client_socket.recv(1024)
    if ("ok" == data.decode()):
        client_socket.send(bytedata)
    data = client_socket.recv(1024)
    if ("ok" == data.decode()):
        pass
print("time consume: " + str(time.time() - startTime))
########################################
result_bytedata = b""
for i in range(len(resultList)):
    result = resultList[i]
    # print(result)
    result_bytedata += (str(result[0])).encode() + ",".encode() + (str(result[1])).encode() + ",".encode() + (str(result[2])).encode() + ",".encode()

bytedata = "result".encode() + ",".encode() + (str(len(result_bytedata))).encode()
client_socket.send(bytedata)
data = client_socket.recv(1024)
if ("ok" == data.decode()):
    pass
# 发送轨迹
client_socket.send(result_bytedata)
data = client_socket.recv(1024)
if ("ok" == data.decode()):
    pass

########################################

resultList = np.array(resultList)
L = getL(resultList)
print("L: " + str(L))

file_handle = open('points.txt', 'w+')
for result in resultList:
    file_handle.write("%f %f %f\n" \
        % (result[0], result[1], result[2]))
file_handle.close()


